package simulator;

import java.awt.BasicStroke;
import java.awt.Color;
import java.awt.Graphics;
import java.awt.Graphics2D;
import java.util.ArrayList;

/**
 * RangeFinder models the behavior of a generic distance sensor (Ultrasonic, IR)
 * providing distance values from object(s) in front of it
 * 
 * @author anand
 * 
 */
public class RangeFinder extends Sensor {
	public static final int HISTORY_SIZE = 7;

	private double range;
	private double[] history;
	private int pointer;
	private double mountangle; // angle at which the RangeFinder with respect to
								// the front of the bot
	// private static final double TOLERANCE = 180.0; //degrees of tolerance in
	// contact
	private Color color = new Color((float) Math.random(),
			(float) Math.random(), (float) Math.random());

	public RangeFinder(Environment e, double x, double y, double range) {
		super(e);
		this.x = x;
		this.y = y;
		this.range = range;
		mountangle = 0;
		history = new double[HISTORY_SIZE];
		pointer = 0;
	}

	public RangeFinder(Environment e, double x, double y, double range,
			double angle) {
		super(e);
		this.x = x;
		this.y = y;
		this.range = range;
		mountangle = Math.toRadians(angle);
		history = new double[HISTORY_SIZE];
		pointer = 0;
	}

	public int getID() {
		return 1;
	}

	public static double deviation(double[] history) {
		double mean = average(history);
		int max = history.length - 1;
		int min = 0;
		for (int i = 0; i < history.length; i++) {
			if (history[i] <= history[min]) {
				min = i;
			} else if (history[i] >= history[max]) {
				max = i;
			}
		}
		double sum = 0;
		for (int j = 0; j < history.length; j++) {
			if (j != max && j != min) {
				sum += ((mean - history[j]) * (mean - history[j]));
			}
		}
		sum = Math.sqrt(sum / (history.length - 2));
		return sum;
	}

	public static double average(double[] history) {
		int max = history.length - 1;
		int min = 0;
		for (int i = 0; i < history.length; i++) {
			if (history[i] <= history[min]) {
				min = i;
			} else if (history[i] >= history[max]) {
				max = i;
			}
		}
		double sum = 0;
		for (int j = 0; j < history.length; j++) {
			if (j != max && j != min) {
				sum += history[j];
			}
		}
		sum /= history.length - 2;
		return sum;
	}

	public double[] read(int magicnum) {
		double[] reading = new double[3];
		reading[0] = range;
		ArrayList<PhysicalObject> known = env.getLocalObjects(
				(env.getRobotX() + x), (env.getRobotY() + y), range);

		for (PhysicalObject o : known) {
			for (int i = 0; i < range; i++) {
				if (o.intersects(
						env.getRobotX()
								+ ((x + i) * (Math.cos(env.getRobotAngle()
										+ mountangle))),
						env.getRobotY()
								+ ((y + i) * (Math.sin(env.getRobotAngle()
										+ mountangle))))) {
					reading[0] = Math.min(i, reading[0]);
					break;
				}
			}

		}
		history[pointer] = reading[0];
		pointer++;
		pointer %= HISTORY_SIZE;
		reading[1] = deviation(history);
		reading[2] = (double) Simulator.time;
		double fil = average(history);
		// System.out.println(reading[0]);
		setState(reading);
		return reading;
	}

	public String getName() {
		return "RangeFinder"; // add id as well?
	}

	public double getRange() {
		return range;
	}

	public void paint(Graphics g) {
		try {
			g.setColor(Color.BLACK);

			Graphics2D g2d = (Graphics2D) g;
			int width = 3;
			g2d.setStroke(new BasicStroke(width));
			g.drawLine(
					(int) (env.getRobotX() + ((x + 5) * Math.cos(env
							.getRobotAngle() + mountangle))),
					(int) (env.getRobotY() + ((y + 5) * Math.sin(env
							.getRobotAngle() + mountangle))),
					(int) (env.getRobotX() + ((x + Math.min(10, getState()[0])) * Math
							.cos(env.getRobotAngle() + mountangle))),
					(int) (env.getRobotY() + ((y + Math.min(10, getState()[0])) * Math
							.sin(env.getRobotAngle() + mountangle))));

			width = 1;
			g2d.setStroke(new BasicStroke(width));
			g.setColor(this.color);
			g.drawLine((int) (env.getRobotX() + ((x + Math.min(10,
					getState()[0])) * Math
					.cos(env.getRobotAngle() + mountangle))), (int) (env
					.getRobotY() + ((y + Math.min(10, getState()[0])) * Math
					.sin(env.getRobotAngle() + mountangle))), (int) (env
					.getRobotX() + ((x + getState()[0]) * Math.cos(env
					.getRobotAngle() + mountangle))),
					(int) (env.getRobotY() + ((y + getState()[0]) * Math
							.sin(env.getRobotAngle() + mountangle))));
		} catch (Exception e) {
			e.printStackTrace(Simulator.out);
		}
	}

	public double getMountAngle() {
		return mountangle;
	}

	public void setMountAngle(double newangle) {
		mountangle = newangle;
	}

}